package Pathall;

import Activemq.Node;
import Redis.Rdo;

import java.util.*;

public class AStarPlanner implements PathPlanner {
    // 方向枚举
    private enum Direction {
        UP(-1, 0),
        DOWN(1, 0),
        LEFT(0, -1),
        RIGHT(0, 1);

        public final int deltaX;
        public final int deltaY;

        Direction(int deltaX, int deltaY) {
            this.deltaX = deltaX;
            this.deltaY = deltaY;
        }
    }

    @Override
    public List<int[]> findPath(int[] start, int[] end, Rdo rdo, int mapWidth, int mapHeight) {
        if (start == null || end == null) return new ArrayList<>();
        if (!isValidCoordinate(end[0], end[1], mapWidth, mapHeight)) return new ArrayList<>();

        // 创建起始节点和目标节点
        Node startNode = new Node(null, start[0], start[1]);
        Node goalNode = new Node(null, end[0], end[1]);

        // 如果起点就是终点
        if (startNode.equals(goalNode)) {
            return Collections.singletonList(new int[]{start[0], start[1]});
        }

        // 初始化数据结构
        PriorityQueue<Node> openList = new PriorityQueue<>();
        Set<String> closedList = new HashSet<>();
        Map<String, Node> nodeCache = new HashMap<>();

        openList.add(startNode);

        while (!openList.isEmpty()) {
            Node currentNode = openList.poll();

            // 找到目标节点
            if (currentNode.equals(goalNode)) {
                return reconstructPath(currentNode);
            }

            String nodeKey = currentNode.getKey();
            if (closedList.contains(nodeKey)) continue;
            closedList.add(nodeKey);

            // 探索所有可能方向
            for (Direction dir : Direction.values()) {
                int neighborX = currentNode.x + dir.deltaX;
                int neighborY = currentNode.y + dir.deltaY;

                // 跳过无效坐标
                if (!isValidCoordinate(neighborX, neighborY, mapWidth, mapHeight)) continue;

                // 关键修改：仅当不是终点时才检查障碍物
                boolean isGoal = (neighborX == goalNode.x && neighborY == goalNode.y);
                if (!isGoal && rdo.checkobstacle(neighborX, neighborY)) {
                    continue;
                }

                String key = neighborX + "," + neighborY;
                Node neighborNode = nodeCache.getOrDefault(key, new Node(null, neighborX, neighborY));

                // 计算移动代价
                double g = currentNode.g + 1;
                double h = heuristic(neighborX, neighborY, goalNode.x, goalNode.y);
                double f = g + h;

                // 如果找到更优路径或新节点
                if (!nodeCache.containsKey(key) || f < neighborNode.f) {
                    neighborNode.g = g;
                    neighborNode.h = h;
                    neighborNode.f = f;
                    neighborNode.parent = currentNode;

                    if (!openList.contains(neighborNode)) {
                        openList.add(neighborNode);
                    }
                    nodeCache.put(key, neighborNode);
                }
            }
        }

        return new ArrayList<>(); // 返回空路径
    }

    // 曼哈顿距离启发函数
    private double heuristic(int x1, int y1, int x2, int y2) {
        return Math.abs(x1 - x2) + Math.abs(y1 - y2);
    }

    // 路径重建方法
    private List<int[]> reconstructPath(Node endNode) {
        List<int[]> path = new ArrayList<>();
        Node currentNode = endNode;
        while (currentNode != null) {
            path.add(0, new int[]{currentNode.x, currentNode.y});
            currentNode = currentNode.parent;
        }
        return path;
    }

    // 坐标有效性检查
    private boolean isValidCoordinate(int x, int y, int mapWidth, int mapHeight) {
        return x >= 0 && x < mapWidth && y >= 0 && y < mapHeight;
    }
}